
(* 
**********************************************************

		 COSY_PAK package chap5.m		

**********************************************************
*)


If [ TrueQ[ $VersionNumber >= 2.0 ],
     Off[ General::spell ];
     Off[ General::spell1 ] ]
     
(*  B E G I N     P A C K A G E  *)


BeginPackage["COSYPAK`chap5`",
		"Graphics`Graphics`"];

RootLocus::usage = 
"RootLocus[Transf, s, {k,kmin,kmax}, gopts]: Plots the root-locus plot of the transfer function Transf(s) with the Parameter k varying from k=kmin to k=kmax. ";

Begin["`Private`"];

(* Plot the Root Locus of a transfer function *)
RootLocus[transf_,s_,{Para_,Paramin_,Paramax_},opts___]:=
Module[{CharPoly,denom,numer,k,klist,poles,npoles,zeros,start,
		g,glist,realline,grealline,sol,linelem,linelist,nlines},
	
	denom = Denominator[transf];
	numer = Numerator[transf];
	
	(* Get the starting pole location *)
	poles=NSolve[denom==0,s];
	realline = Cases[s/.poles, x_/;(Abs[Im[x]] < 0.0001) ]; 
	poles=Map[{Re[#],Im[#]}& ,s/.poles];
		
	poles=Table[Append[poles[[i]],"X"],{i,1,Length[poles]}];
	npoles=Length[poles];
	glist = Array[g,npoles+1,0];
	start = poles;

	
	(* If numerator exists *)	
	If[! FreeQ[numer,s],    
		
		(* Produce the mark of zeros *)
		zeros=NSolve[numer==0,s];
		realline = Join[realline,Cases[s/.zeros, x_/;(Abs[Im[x]]<0.0001)]];  
		zeros=Map[{Re[#],Im[#]}& ,s/.zeros];
		zeros=Table[Append[zeros[[i]],"O"],{i,1,Length[zeros]}];
		start=Join[poles,zeros]
		];
	
	(* Procedure to produce lines to push root locus to reach zero *) 
	nlines = Ceiling[Length[realline]/2];
   	realline = Sort[realline,Greater];
    	linelist = Array[linelem, nlines ];
    
    	If[EvenQ[Length[realline]],     
   	  	Do[ linelem[i] = Line[ {{realline[[2 i-1]],0},{realline[[2 i]],0}}],
    		{i,1, nlines}],
    		
      		Do[ linelem[i] = Line[ {{realline[[2 i-1]],0},{realline[[2 i]],0}} ],
            	{i,1, nlines-1}];
      
      	linelem[nlines] = 
      	Line[{{realline[[2 nlines-1]],0},{realline[[2 nlines -1]] -2 ,0}}] 
   		
    	];
  	
	linelist = Prepend[linelist,Thickness[0.0051]];
	grealline = Graphics[linelist]; 
	CharPoly = Collect[Numerator[Together[1 + transf Para]],s];
	
	(* Looking for positive break away or break in gain *)
	keqn = Para/.Solve[CharPoly == 0,Para][[1]];
	sol = NSolve[Numerator[Together[D[keqn,s]]]==0,s];

	(* Pick up positive real parameter at the breaking point *)
	klist = Cases[keqn/.sol,x_/;(Re[x]>0 && Abs[Im[x]] < 0.0001) ];

	(* Producing the parameter listing for plotting *)
	k = Paramin;
	a = 0.08;b = 0.03;
	While[k <= Paramax ,klist = Append[klist,k]; k = k (1 + a) + b];

	(* Appedn the final value of k then sort the klist *)
	klist = Sort[Append[klist,Paramax]];
	EqnSolve[x_]:= NSolve[ (CharPoly/.Para->x) == 0 ,s];
	points = Map[{Re[#],Im[#]}& , s/.Map[EqnSolve,klist] ,{2}]; 
	(* Graph of Poles and Zeros position *)
	g[0] = TextListPlot[start,DisplayFunction -> Identity];


	(* Graph Sections of Root Locus devided by break points *)
	Do[ g[j] = ListPlot[ Table[points[[i,j]],{i,1,Length[klist]}],
			PlotJoined->True,PlotStyle->{Thickness[0.0051]},
			DisplayFunction -> Identity ],
		{j,1,npoles}
	  ];
	(* Show all graphics *)	
	Show[{grealline,glist},
		DisplayFunction -> $DisplayFunction,PlotRange -> All,
		FrameLabel->{"Re","Im","Root Locus"," "},Frame->True,
		Ticks->{Automatic, Automatic},
		GridLines->Automatic, 
        FrameTicks -> {Automatic, Automatic,Automatic, Automatic},opts]
	]/; Paramin < Paramax
		
End[];
EndPackage[];

(*  E N D     P A C K A G E  *)

If [ TrueQ[ $VersionNumber >= 2.0 ],
     On[ General::spell ];
     On[ General::spell1 ] ]

